#ifndef PHITRON_H
#define PHITRON_H

#include <vector>
#include "vector.h"
#include "link.h"

namespace phitron {
    struct Force {
        double attraction;
        double cross;
    };

    template <int amnt>
    struct Laws {
        double friction;
        Force forces[amnt][amnt];
    };

    template <int amnt>
    struct Object {
        double inertia;
        double quantae[amnt];

        Vector3 position;
        Vector3 velocity;
        Vector3 acceleration;

        Object() {}

        Object(double inertia, Vector3 position, Vector3 velocity = Vector3::zero, double* quantae = nullptr) {
            if (quantae)
                for (int i = 0; i < amnt; i++)
                    this->quantae[i] = quantae[i];
            this->inertia = inertia;
            this->position = position;
            this->velocity = velocity;
            this->acceleration = Vector3::zero;
        }

        inline Vector3 force(Object<amnt>& other, Laws<amnt>& laws, Vector3 offset = Vector3::zero) {
            //Get the distance vector
            Vector3 attract = offset + other.position - position;
            //Find the magnitude of the distance
            double distance = attract.magnitude();
            //Divide by distance cubed to recieve 1/distance^2
            attract /= distance*distance*distance;

            //Calculate the cross vector using relative velocity
            Vector3 cross = attract / (other.velocity - velocity);

            double attractMagnitude = 0;
            double crossMagnitude = 0;
            //Loop through all forces
            for (int i = 0; i < amnt; i++)
                    for (int j = 0; j < amnt; j++) {
                            double quantaeProduct = other.quantae[i] * quantae[j];
                            attractMagnitude += quantaeProduct * laws.forces[i][j].attraction;
                            crossMagnitude += quantaeProduct * laws.forces[i][j].cross;
                    }

            //Multiply attract and cross vector by their respective constant coefficients
            return attract * attractMagnitude + cross * crossMagnitude;
        }
    };

    template <int amnt>
    struct Group {
        Link<Object<amnt>>* objects;

        Group () {
            objects = nullptr;
        }

        //Calculate only internal forces
        void forces(Laws<amnt>& laws) {
            for (Link<Object<amnt>>* link = objects; link; link = link->next) {
                //Add in friction
                link->object.acceleration -= ink->object.velocity * laws.friction;

                //Apply forces from and to all objects after this one in this group
                for (Link<Object<amnt>>* other = link->next; other; other = other->next) {
                    Vector3 force = link->object.force(other->object, laws);
                    link->object.acceleration += force / link->object.inertia;
                    other->object.acceleration -= force / other->object.inertia;
                }
            }
        }

        //Add in acceleration caused by external objects or set it to nullptr to calculate internal acceleration
        void forces(Laws<amnt>& laws, Vector3 offset, Link<Object<amnt>>* external = nullptr) {
            //If external is null
            if (!external)
                //Use this group as the external group
                external = objects;

            //Loop through everything in this group
            for (Link<Object<amnt>>* link = objects; link; link = link->next)
                //Loop through everything in the external group
                for (Link<Object<amnt>>* other = external; other; other = other->next)
                    //Add the acceleration on this group's object
                    link->object.acceleration += link->object.force(other->object, laws, offset) / link->object.inertia;
        }

        //Accelerate the objects in this group using dt
        void accelerate(double dt) {
            for (Link<Object<amnt>>* link = objects; link; link = link->next)
                link->object.velocity += link->object.acceleration * dt;
        }

        //Move the objects
        void move(double dt) {
            for (Link<Object<amnt>>* link = objects; link; link = link->next)
                link->object.position += link->object.velocity * dt;
        }

        //Add a SINGLE new object link into this group
        void add(Link<Object<amnt>>* link) {
            objects = link->setNext(objects);
        }

        //Update velocity and position, then reset acceleration
        void update(double dt) {
            accelerate(dt);
            move(dt);
            for (Link<Object<amnt>>* link = objects; link; link = link->next)
                link->object.acceleration = Vector3::zero;
        }
    };
}

#endif // PHITRON_H

